
#include "bottom_canfd.h"
#include "stm32h7xx_hal_fdcan.h"
#include "MyIncludes.h"

// CAN handle definitions
FDCAN_HandleTypeDef Can1Handle;
FDCAN_HandleTypeDef Can2Handle;

// Global message structures
FDCAN_RxHeaderTypeDef RxHeader;
CAN_msg CAN1_RxMsg;
CAN_msg CAN2_RxMsg;
uint16_t CAN1_RxRdy = 0;
uint16_t CAN2_RxRdy = 0;

// MSP Initialization for FDCAN1 and FDCAN2
void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef *hfdcan)
{
    GPIO_InitTypeDef GPIO_InitStruct = {0};
    RCC_PeriphCLKInitTypeDef RCC_PeriphClkInit = {0};

    if (hfdcan->Instance == FDCAN1) {
        // Enable GPIO clocks for CAN1
        FDCANx_TX_GPIO_CLK_ENABLE();
        FDCANx_RX_GPIO_CLK_ENABLE();

        // Configure FDCAN clock source (PLL1Q)
        RCC_PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_FDCAN;
        RCC_PeriphClkInit.FdcanClockSelection = RCC_FDCANCLKSOURCE_PLL;
        HAL_RCCEx_PeriphCLKConfig(&RCC_PeriphClkInit);

        // Enable FDCAN1 clock
        FDCANx_CLK_ENABLE();
        FDCANx_FORCE_RESET();
        FDCANx_RELEASE_RESET();

        // Configure TX pin
        GPIO_InitStruct.Pin = FDCANx_TX_PIN;
        GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
        GPIO_InitStruct.Pull = GPIO_PULLUP;
        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
        GPIO_InitStruct.Alternate = FDCANx_TX_AF;
        HAL_GPIO_Init(FDCANx_TX_GPIO_PORT, &GPIO_InitStruct);

        // Configure RX pin
        GPIO_InitStruct.Pin = FDCANx_RX_PIN;
        GPIO_InitStruct.Alternate = FDCANx_RX_AF;
        HAL_GPIO_Init(FDCANx_RX_GPIO_PORT, &GPIO_InitStruct);

        // Configure NVIC for FDCAN1
        HAL_NVIC_SetPriority(FDCANx_IT0_IRQn, CAN_interrupt_priority, 0);
        HAL_NVIC_SetPriority(FDCANx_IT1_IRQn, CAN_interrupt_priority, 0);
        HAL_NVIC_SetPriority(FDCAN_CAL_IRQn, CAN_interrupt_priority, 0);
        HAL_NVIC_EnableIRQ(FDCANx_IT0_IRQn);
        HAL_NVIC_EnableIRQ(FDCANx_IT1_IRQn);
        HAL_NVIC_EnableIRQ(FDCAN_CAL_IRQn);
    }
    else if (hfdcan->Instance == FDCAN2) {
        // Enable GPIO clocks for CAN2
        CAN2_GPIO_CLK_ENABLE();

        // Enable FDCAN2 clock (shared clock with FDCAN1)
        // FDCANx_CLK_ENABLE();
        // FDCANx_FORCE_RESET();
        // FDCANx_RELEASE_RESET();

        // Configure TX pin
        GPIO_InitStruct.Pin = CAN2_TX_PIN;
        GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
        GPIO_InitStruct.Pull = GPIO_PULLUP;
        GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
        GPIO_InitStruct.Alternate = GPIO_AF9_FDCAN2;
        HAL_GPIO_Init(CAN2_TX_GPIO_PORT, &GPIO_InitStruct);

        // Configure RX pin
        GPIO_InitStruct.Pin = CAN2_RX_PIN;
        GPIO_InitStruct.Alternate = GPIO_AF9_FDCAN2;
        HAL_GPIO_Init(CAN2_RX_GPIO_PORT, &GPIO_InitStruct);

        // Configure NVIC for FDCAN2
        HAL_NVIC_SetPriority(FDCAN2_IT0_IRQn, CAN_interrupt_priority, 0);
        HAL_NVIC_SetPriority(FDCAN2_IT1_IRQn, CAN_interrupt_priority, 0);
        HAL_NVIC_SetPriority(FDCAN_CAL_IRQn, CAN_interrupt_priority, 0);
        HAL_NVIC_EnableIRQ(FDCAN2_IT0_IRQn);
        HAL_NVIC_EnableIRQ(FDCAN2_IT1_IRQn);
        HAL_NVIC_EnableIRQ(FDCAN_CAL_IRQn);
    }
}

// Initialize FDCAN1
void FDCAN1_Init(CAN_ModeTypeDef Mode, uint32_t tsjw, uint32_t tbs1, uint32_t tbs2, uint16_t brp)
{
    Can1Handle.Instance = FDCAN1;
    Can1Handle.State = HAL_FDCAN_STATE_RESET;
    Can1Handle.Init.FrameFormat = CAN_FRAME_CLASSIC;
    Can1Handle.Init.Mode = Mode;
    Can1Handle.Init.AutoRetransmission = ENABLE;
    Can1Handle.Init.TransmitPause = DISABLE;
    Can1Handle.Init.ProtocolException = ENABLE;
    Can1Handle.Init.NominalPrescaler = brp;
    Can1Handle.Init.NominalSyncJumpWidth = tsjw;
    Can1Handle.Init.NominalTimeSeg1 = tbs1;
    Can1Handle.Init.NominalTimeSeg2 = tbs2;
    Can1Handle.Init.DataPrescaler = brp;
    Can1Handle.Init.DataSyncJumpWidth = CAN_DEFAULT_DATA_SJW;
    Can1Handle.Init.DataTimeSeg1 = CAN_DEFAULT_DATA_TS1;
    Can1Handle.Init.DataTimeSeg2 = CAN_DEFAULT_DATA_TS2;
    Can1Handle.Init.MessageRAMOffset = 0;
    Can1Handle.Init.StdFiltersNbr = 1;
    Can1Handle.Init.ExtFiltersNbr = 0;
    Can1Handle.Init.RxFifo0ElmtsNbr = 2;
    Can1Handle.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;
    Can1Handle.Init.RxFifo1ElmtsNbr = 0;
    Can1Handle.Init.RxBuffersNbr = 0;
    Can1Handle.Init.TxEventsNbr = 0;
    Can1Handle.Init.TxBuffersNbr = 0;
    Can1Handle.Init.TxFifoQueueElmtsNbr = 32;
    Can1Handle.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
    Can1Handle.Init.TxElmtSize = FDCAN_DATA_BYTES_8;

    HAL_FDCAN_Init(&Can1Handle);

    // Configure filter for CAN1
    FDCAN_FilterTypeDef sFilterConfig = {
        .IdType = FDCAN_STANDARD_ID,
        .FilterIndex = 0,
        .FilterType = FDCAN_FILTER_MASK,
        .FilterConfig = FDCAN_FILTER_TO_RXFIFO0,
        .FilterID1 = CAN1_DEFAULT_ID,
        .FilterID2 = CAN_FILTER_MASK
    };
    HAL_FDCAN_ConfigFilter(&Can1Handle, &sFilterConfig);

    HAL_FDCAN_ConfigFifoWatermark(&Can1Handle, FDCAN_CFG_RX_FIFO0, 1);
    HAL_FDCAN_ActivateNotification(&Can1Handle, FDCAN_IT_RX_FIFO0_WATERMARK, 0);
    HAL_FDCAN_Start(&Can1Handle);
}

// Initialize FDCAN2
void FDCAN2_Init(CAN_ModeTypeDef Mode, uint32_t tsjw, uint32_t tbs1, uint32_t tbs2, uint16_t brp)
{
    Can2Handle.Instance = FDCAN2;
    Can2Handle.State = HAL_FDCAN_STATE_RESET;
    Can2Handle.Init.FrameFormat = CAN_FRAME_CLASSIC;
    Can2Handle.Init.Mode = Mode;
    Can2Handle.Init.AutoRetransmission = ENABLE;
    Can2Handle.Init.TransmitPause = DISABLE;
    Can2Handle.Init.ProtocolException = ENABLE;
    Can2Handle.Init.NominalPrescaler = brp;
    Can2Handle.Init.NominalSyncJumpWidth = tsjw;
    Can2Handle.Init.NominalTimeSeg1 = tbs1;
    Can2Handle.Init.NominalTimeSeg2 = tbs2;
    Can2Handle.Init.DataPrescaler = brp;
    Can2Handle.Init.DataSyncJumpWidth = CAN_DEFAULT_DATA_SJW;
    Can2Handle.Init.DataTimeSeg1 = CAN_DEFAULT_DATA_TS1;
    Can2Handle.Init.DataTimeSeg2 = CAN_DEFAULT_DATA_TS2;
    Can2Handle.Init.MessageRAMOffset = 0;
    Can2Handle.Init.StdFiltersNbr = 1;
    Can2Handle.Init.ExtFiltersNbr = 0;
    Can2Handle.Init.RxFifo0ElmtsNbr = 2;
    Can2Handle.Init.RxFifo0ElmtSize = FDCAN_DATA_BYTES_8;
    Can2Handle.Init.RxFifo1ElmtsNbr = 0;
    Can2Handle.Init.RxBuffersNbr = 0;
    Can2Handle.Init.TxEventsNbr = 0;
    Can2Handle.Init.TxBuffersNbr = 0;
    Can2Handle.Init.TxFifoQueueElmtsNbr = 32;
    Can2Handle.Init.TxFifoQueueMode = FDCAN_TX_FIFO_OPERATION;
    Can2Handle.Init.TxElmtSize = FDCAN_DATA_BYTES_8;

    HAL_FDCAN_Init(&Can2Handle);

    // Configure filter for CAN2
    FDCAN_FilterTypeDef sFilterConfig = {
        .IdType = FDCAN_STANDARD_ID,
        .FilterIndex = 0,
        .FilterType = FDCAN_FILTER_MASK,
        .FilterConfig = FDCAN_FILTER_TO_RXFIFO0,
        .FilterID1 = CAN2_DEFAULT_ID,
        .FilterID2 = CAN_FILTER_MASK
    };
    HAL_FDCAN_ConfigFilter(&Can2Handle, &sFilterConfig);

    HAL_FDCAN_ConfigFifoWatermark(&Can2Handle, FDCAN_CFG_RX_FIFO0, 1);
    HAL_FDCAN_ActivateNotification(&Can2Handle, FDCAN_IT_RX_FIFO0_WATERMARK, 0);
    HAL_FDCAN_Start(&Can2Handle);
}

// Send message via specified CAN handle
void FDCAN_SendMessage(FDCAN_HandleTypeDef *hfdcan, uint32_t id, uint8_t *buff, uint32_t len)
{
// 等待 TX FIFO 有空闲槽位
    while (HAL_FDCAN_GetTxFifoFreeLevel(hfdcan) == 0) {
        HAL_Delay(1);  // 短暂延时，避免忙等待
    }

    FDCAN_TxHeaderTypeDef TxHeader = {
        .Identifier = id,
        .IdType = FDCAN_STANDARD_ID,
        .TxFrameType = FDCAN_DATA_FRAME,
        .DataLength = (len << 16),  // 转换为 FDCAN 长度格式
        .ErrorStateIndicator = FDCAN_ESI_ACTIVE,
        .BitRateSwitch = FDCAN_BRS_ON,
        .FDFormat = FDCAN_CLASSIC_CAN,
        .TxEventFifoControl = FDCAN_NO_TX_EVENTS,
        .MessageMarker = 0
    };

    if (HAL_FDCAN_AddMessageToTxFifoQ(hfdcan, &TxHeader, buff) != HAL_OK) {
        SEGGER_RTT_CustomFunction(0, OUTPUT_WINDOW_NUW, "Failed to send CAN message\n");
    }
}

// Receive message callback
void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs)
{
    if ((RxFifo0ITs & FDCAN_IT_RX_FIFO0_WATERMARK) != RESET) {
        uint8_t RxData[8];
        HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &RxHeader, RxData);

        char buffer[50];
        int can_num = (hfdcan == &Can1Handle) ? 1 : 2;
        snprintf(buffer, sizeof(buffer), "CAN%d Rx: ID=0x%lX, Data=", can_num, RxHeader.Identifier);
        for (uint32_t i = 0; i < (RxHeader.DataLength >> 16); i++) {
            snprintf(buffer + strlen(buffer), sizeof(buffer) - strlen(buffer), "%02X ", RxData[i]);
        }
        SEGGER_RTT_CustomFunction(0, OUTPUT_WINDOW_NUW, "%s\n", buffer);
        // 最大频率 3000 Hz，加速度 500 Hz/s，总步数 1000			
        //Stepper_Trapezoidal_Control(2000.0f, 500.0f, 300); 
        TIM8_CH1_PWM_Control(200000000 , 300 , 0.5, 1000);	

		//使用梯形加减速控制替代原来的PWM控制
        // 参数：启动频率=10Hz, 最大频率=3000Hz, 结束频率=10Hz, 加速度=500Hz/s, 总脉冲=1000, 占空比=50%
//        TIM8_Trapezoidal_Init(10.0f, 100.0f, 10.0f, 100.0f, 100, 0.5f);
//        TIM8_Trapezoidal_Start();
				
        if (hfdcan == &Can1Handle) {
            CAN1_RxMsg.data = RxData;
            CAN1_RxRdy = 1;
        } else if (hfdcan == &Can2Handle) {
            CAN2_RxMsg.data = RxData;
            CAN2_RxRdy = 1;
        }

        HAL_FDCAN_ActivateNotification(hfdcan, FDCAN_IT_RX_FIFO0_WATERMARK, 0);
    }
}

/**
  * @brief This function handles FDCAN1 interrupt 0.
  */
void FDCAN1_IT0_IRQHandler(void)
{
    HAL_FDCAN_IRQHandler(&Can1Handle);
}

/**
  * @brief This function handles FDCAN1 interrupt 1.
  */
void FDCAN1_IT1_IRQHandler(void)
{
    HAL_FDCAN_IRQHandler(&Can1Handle);
}

/**
  * @brief This function handles FDCAN2 interrupt 1.
  */
void FDCAN2_IT1_IRQHandler(void)
{
  HAL_FDCAN_IRQHandler(&Can2Handle);
}
/**
  * @brief This function handles FDCAN2 interrupt 0.
  */
void FDCAN2_IT0_IRQHandler(void)
{
  HAL_FDCAN_IRQHandler(&Can2Handle);
}

/**
  * @brief This function handles FDCAN calibration unit interrupt.
  */
void FDCAN_CAL_IRQHandler(void)
{
    HAL_FDCAN_IRQHandler(&Can1Handle);
    HAL_FDCAN_IRQHandler(&Can2Handle);
}

// void CAN2_RX1_IRQHandler(void)
// {
//     HAL_FDCAN_IRQHandler(&Can2Handle);
// }


